/**
 * @file cubic_interpolation.c
 * @brief Cubic interpolation library.
 * 
 * More information about cubic interpolation in one and more dimensions
 * you can find online at: http://www.paulinternet.nl/?page=bicubic .
 * @author Dobrin Georgiev
 * @date 2013.10.26
 */
#include <stdio.h>	// fprint
#include <stdlib.h>	// calloc
#include <stdint.h>
#include <math.h>	// exp
#include "cubic_interpolation.h"

/**
 * @brief Cubic integrator
 * @param [in] p Four data points. Keep in mind that actual interpolation region is [p[1] - p[2]].
 * @param [in] x Position of interpolated data [0 - 1].
 * @return Interpolated data.
 *
 * Cubic integrator core function.
 */
static float cubic_integrator(float *p, double x)
{
	float a, b, c, d, y;
	uint8_t i;

	for (i=0;i<CUBIC_INTER_POINTS;i++) {
		if (NULL == p+i) {
			return(0.f);
		}
	}

	a = -.5f*p[0] + 1.5f*p[1] -1.5f*p[2] + .5f*p[3];
	b = p[0] -2.5f*p[1] + 2*p[2] - .5f*p[3];
	c = -.5f*p[0] + .5f*p[2];
	d = p[1];

	y = a*x*x*x + b*x*x + c*x +d;

	return(y);
}
/**
 * @brief Curve re-sample, up and down scale.
 * @param [in] in Input curve
 * @param [out] out Re-sampled curve
 */
void curve_resample(curve_t in, curve_t out)
{
#define FIRST			0
#define LAST			(CUBIC_INTER_POINTS-1)
#define IN_FIRST		0
#define IN_SECOND		1
#define IN_SECOND_TO_LAST	(in.size-2)
#define IN_LAST			(in.size-1)
	float p[CUBIC_INTER_POINTS];
	float ratio;
	uint8_t i, j;

	if (NULL == in.data)
		return;
	if (NULL == out.data)
		return;
	if (CUBIC_INTER_MIN_DATA > in.size)
		return;

	ratio = (float)(out.size-1) / (in.size-1);
//	printf("ratio = %f\n", ratio);
	out.data[0] = in.data[0];
	out.data[out.size-1] = in.data[in.size-1];
	for (i=1;i<out.size-1;i++) {
		uint8_t index = i/ratio;	// in index
//		printf("index = %d\n", index);
		for (j=0;j<CUBIC_INTER_POINTS;j++) {
			if (FIRST == j) {
				if (0 > index-1) { // if there is not enough data points
					p[FIRST] = 2*in.data[IN_FIRST] - in.data[IN_SECOND];
//					printf("extrapolate first\n");
					continue;
				}
			}
			if (LAST == j) {
				if (in.size <= index+2) { // if there is not enough data points
					p[LAST] = 2*in.data[IN_LAST] - in.data[IN_SECOND_TO_LAST];
//					printf("extrapolate last\n");
					continue;
				}
			}
			p[j] = in.data[index-1+j];
		}
		out.data[i] = cubic_integrator(p, (float)i/ratio-index);
	}
	return;
}
/**
 * @brief Surface re-sample, up and down scale.
 * @param [in] in Input surface
 * @param [out] out Re-sampled surface
 */
void surface_resample(surface_t in, surface_t out)
{
	surface_t tmp;
	uint8_t i, j;

	if (NULL == in.data)
		return;
	if (NULL == out.data)
		return;
	if (CUBIC_INTER_MIN_DATA > in.row)
		return;
	if (CUBIC_INTER_MIN_DATA > in.col)
		return;

	tmp.row = in.row;
	tmp.col = out.col;
	tmp.data = calloc(tmp.row * tmp.col, sizeof(float));
	if (NULL == tmp.data)
		return;

	// resample rows
	for (i=0;i<in.row;i++) {
		curve_t c_in = {.data = in.data + i*in.col, .size = in.col};
		curve_t c_out = {.data = tmp.data + i*tmp.col, .size = tmp.col};
		curve_resample(c_in, c_out);
	}
	// resample cols
	float *in_data = calloc(tmp.row, sizeof(float));
	float *out_data = calloc(out.row, sizeof(float));
	for (i=0;i<out.col;i++) {
		for (j=0;j<tmp.row;j++) {
			in_data[j] = *(tmp.data+j*tmp.col+i);
		}
		curve_t c_in = {.data = in_data, .size = tmp.row};
		curve_t c_out = {.data = out_data, .size = out.row};
		curve_resample(c_in, c_out);
		for (j=0;j<out.row;j++) {
			*(out.data+j*out.col+i) = *(c_out.data+j);
		}
	}
	free(out_data);
	out_data = NULL;
	free(in_data);
	in_data = NULL;
	free(tmp.data);
	tmp.data = NULL;

	return;
}
/**
 * @brief Gaussian "bell" surface.
 * @param [in] x X coordinate.
 * @param [in] y Y coordinate.
 * @return Value from Gaussian "bell" corresponding to the point (x, y).
 */
static float gauss(float x, float y)
{
	float g;

	g = exp(-(x*x + y*y)/2);
	//printf("[%6.3f][%6.3f]=%6.3f\n", x, y, g);
	return(g);
}
/**
 * @brief Initialize selected surface with Gaussian "bell".
 * @param [in] s Surface to initialize.
 * @note X and Y ranges are hard coded to [-1; +1].
 */
void surface_gauss_init(surface_t s)
{
#define GAUSS_X_MIN	(-1)
#define GAUSS_X_MAX	(+1)
#define GAUSS_X_RANGE	(GAUSS_X_MAX - GAUSS_X_MIN)
#define GAUSS_Y_MIN	(-1)
#define GAUSS_Y_MAX	(+1)
#define GAUSS_Y_RANGE	(GAUSS_Y_MAX - GAUSS_Y_MIN)
	uint8_t i, j;
	float *p;
	float x, y;

	if (NULL == s.data) {
		return;
	}
	for (i=0;i<s.row;i++) {
		for (j=0;j<s.col;j++) {
			x = GAUSS_X_RANGE/(float)(s.col-1)*j + GAUSS_X_MIN;	// [-1;+1]
			y = GAUSS_Y_RANGE/(float)(s.row-1)*i + GAUSS_Y_MIN;	// [-1;+1]
			p = s.data + i*s.col + j;
			*p = gauss(x, y);
		}
	}
}
/**
 * @brief Print out all surface nodes.
 * @param [in] s Surface to print
 */
void surface_print(surface_t s)
{
	uint8_t i, j;
	float *p;

	if (NULL == s.data) {
		return;
	}
	printf("\"%s\"(%dx%d) surface:\n", s.name, s.row, s.col);
	for (i=0;i<s.row;i++) {
		for (j=0;j<s.col;j++) {
			p = s.data + i*s.col + j;
			printf("%s%+7.4f", j ? ", " : "", *p); 
		}
		printf("\n");
	}
}
/**
 * @brief Find difference between two surfaces.
 * Basically this is matrix subtraction.
 * Matrix connected to surface B is subtracted
 * from matrix connected to surface A,
 * result is stored as to new surface D.
 * @param [out] d Surface to keep the result.
 * @param [in] a Input surface.
 * @param [in] b Input surface.
 */
void surface_diff(surface_t d, surface_t a, surface_t b)
{
	uint8_t i, j;
	uint16_t k;

	if (NULL == d.data) {
		return;
	}
	if (NULL == a.data) {
		return;
	}
	if (NULL == b.data) {
		return;
	}
	if ((d.row != a.row) || (d.row != b.row)) {
		return;
	}
	if ((d.col != a.col) || (d.col != b.col)) {
		return;
	}
	for (i=0;i<d.row;i++) {
		for (j=0;j<d.col;j++) {
			k = i*d.col + j;
			d.data[k] = a.data[k] - b.data[k];
		}
	}
}
